#include "gyro.h"

static float start_yaw = 0;

float Gyro_Analysis(uint8_t* buffer,float New_Ang)  // 解析陀螺仪数据
{
	static float yaw = 0;
	static uint8_t start = 0;
	static int8_t flag=0;	
	static float Last_yaw = 0;
	
	yaw=(float)((buffer[10]&0x0f)*100)+((buffer[11]>>4)*10)+(buffer[11]&0x0f)
		+((buffer[12]>>4)*0.1)+((buffer[12]&0x0f)*0.01);
	if (buffer[10]>>4 == 1)
		yaw = -yaw;
	
	// 角度归一化
	if (yaw > 180.f)
		yaw -= 360.f;
	else if (yaw < -180.f)
		yaw += 360.f;

	if ((start == 0 && yaw != 0 )||(New_Ang !=0 && start == 1))
	{
		start_yaw = yaw + New_Ang;
		start = 1;
		if(New_Ang != 0)
			start = 2;
	}
	yaw = yaw - start_yaw;
	
	if(yaw-Last_yaw<-180.f)  	// 多圈计数
		flag++;
	else if(yaw-Last_yaw>180.f)
		flag--;
	
	if(fabs(yaw - Last_yaw)>360.f)  // 防止跳变
		yaw = Last_yaw;
	
    Last_yaw = yaw;
	
	return yaw+flag*360.f;
}
